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Abstract 


World  modeling  for  achieving  operational  space  motion  control  of  robot  arms  requires 
accurate  measurements  of  positions  and  velocities  in  both  joint  and  operational  space. 
Servomotors  used  for  joint  actuation  are  normally  equipped  with  position  sensors  and, 
optionally,  with  velocity  sensors  for  interlink  motion  measurements.  Further  improvements  in 
measurement  accuracy  can  be  obtained  by  equipping  the  robot  arm  with  accelerometers  for 
absolute  acceleration  measurement.  In  this  report  an  Extended  Kalman  Filter  is  used  for 
multi-sensor  fusion.  The  real-time  control  algorithm  was  previously  based  on  the  assumption 
of  a  jerk  represented  as  a  white  noise  process  with  zero  mean.  In  reality,  the  accelerations  are 
varying  in  time  during  the  arm  motion  and  the  zero  mean  assumption  is  not  valid,  particularly 
during  periods  of  fast  acceleration.  In  this  report,  a  model  predictive  control  approach  is  used 
for  predetermining  next-time-step  jerk  such  that  the  remaining  term  can  be  modeled  as 
Gaussian  white  noise.  Experimental  results  illustrate  the  effectiveness  of  the  proposed  sensor 
fusion  approach. 
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Resume 


La  moderation  de  l’environnement  pour  le  controle  du  mouvement  dans  l’espace  a  besoin 
de  mesures  precises  de  la  position  et  de  la  vitesse  tant  dans  l’espace  des  articulations  que  dans 
l’espace  operationnelle.  Les  servomoteurs  utilises  pour  les  actionneurs  des  articulations  sont 
normalement  equipes  avec  des  capteurs  de  position  et,  en  option,  avec  des  capteurs  de  vitesse 
pour  la  mesure  du  mouvement  entre  les  composants  du  bras  de  robot.  Des  ameliorations 
supplementaires  de  la  precision  de  mesure  peuvent  etre  obtenues  en  installant  sur  le  bras  du 
robot  des  accelerometres  pour  la  mesure  de  l’acceleration  absolue. 

Dans  ce  rapport  est  utilise  un  Filtre  Kalman  Elargie  pour  la  fusion  des  capteurs  multiples. 
L’algorithme  de  controle  en  temps-reel  a  ete,  dans  le  passe,  basee  sur  l’hypothese  que  la 
secousse  est  representee  comme  un  processus  de  bruit  blanc  avec  moyenne  zero.  En  realite, 
les  accelerations  varient  en  temps  pendent  le  mouvement  du  bras  et  l’hypothese  de  moyenne 
zero  n’est  pas  correcte,  en  particulier  pendent  des  periodes  d’ acceleration  rapide.  Dans  ce 
rapport,  une  approche  de  commande  predictive  basee  sur  un  module  est  employee  pour  le 
precalcul  de  la  secousse  du  prochain  pas  de  temps  d’une  telle  facon  que  la  composante 
restante  peut  etre  representee  comme  bruit  blanc  gaussiene.  Des  resultats  d’ experience 
illustrent  l’efficacite  de  l’approche  proposee  pour  la  fusion  des  capteurs. 
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Executive  summary 


Operational  space  motion  control  of  non-linear  dynamic  system,  such  as  robot  arm  requires 
accurate  measurements  of  positions  and  velocities  in  both  joint  and  operational  space. 
Servomotors  used  for  joint  actuation  are  normally  equipped  with  position  sensors  and, 
optionally,  with  velocity  sensors  for  interlink  motion  measurements.  Further  improvements  in 
measurement  accuracy  can  be  obtained  by  equipping  the  robot  arm  with  accelerometers  for 
absolute  acceleration  measurement. 

The  implementation  of  the  control  system  such  as  an  impedance  controller  requires 
operational  space  linearization  based  on  the  computed  torque  approach.  Extensive  work  has 
been  carried  out  to  improve  the  computed  torque  approach  for  robot  motion  control  by  using 
different  types  of  parameter  identification  methods,  by  modelling  the  unknown  dynamics,  and 
by  developing  more  robust  controllers.  In  general,  analytically  computed  torque  has  been 
often  considered  computationally  prohibitive  for  real  time  controller  implementation. 

In  this  paper,  further  improvements  to  the  operational  space  impedance  control  are  proposed 
using  various  computationally  efficient  methods.  First,  a  perturbation  observer  is  used  to 
obtain  an  approximation  of  the  joint  linearization  torque  value.  The  perturbation  observer 
uses  a  combination  of  time  delay  and  acceleration  tracing  methods  for  estimating  non-inertial 
torques.  The  perturbation  observer  requires  an  accurate  joint  acceleration  estimation  obtained 
from  measurements  and  computations.  The  computation  of  acceleration  by  numerical 
derivation  of  the  joint  position  is  possible  due  to  the  availability  of  high  resolution  joint 
position  sensors.  Joint  acceleration  can  be  obtained  by  kinematic  computations  using  the 
measurement  of  absolute  acceleration.  Combining  the  end-effector  accelerometer  signals  and 
joint  position  sensors  signals  can  give  an  acceleration  estimate  with  higher  bandwidth  than 
each  separately. 

To  improve  the  accuracy  and  bandwidth  of  the  signals,  an  Extended  Kalman  Filter  is  used 
for  multi-sensor  fusion.  The  real-time  control  algorithm  was  previously  based  on  the 
assumption  of  a  jerk  represented  as  a  processed  white  noise  with  zero  mean.  In  reality,  the 
accelerations  are  varying  in  time  during  the  arm  motion  and  the  zero  mean  assumption  is  not 
valid,  particularly  during  periods  of  fast  accelerating.  In  this  report,  a  model  predictive 
control  approach  is  used  for  predetermining  next-time-step  jerk  such  that  the  remaining  term 
can  be  modeled  as  Gaussian  white  noise. 

The  experimental  results  shown  in  this  report,  illustrates  the  effectiveness  of  the  proposed 
sensor  fusion  approach  (Extended  Kalman  Filter)  in  real  time. 


Jassemi-Zargani,  R.;  Necsulescu,  D.  2001.  Extended  Kalman  Filter  Sensor  Fusion  signals 
of  Nonlinear  Dynamic  Systems.  DREO  TR  2001-155,  Defence  Research  Establishment 
Ottawa. 
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Sommaire 


Le  controle  du  mouvement  dans  l’espace  operationnel  d’un  systeme  nonlinaire,  comme  par 
example  un  bras  de  robot,  a  besoin  de  mesures  precises  de  la  position  et  de  la  vitesse  tant  dans 
l’espace  des  articulations  que  dans  l’espace  operationnel.  Les  servomoteurs  utilises  pour  les 
actionneurs  des  articulations  sont  normalement  equipds  avec  des  capteurs  de  position  et,  en 
option,  avec  des  capteurs  de  vitesse  pour  la  mesure  du  mouvement  entre  les  composants  du 
bras  de  robot.  Des  ameliorations  supplementaires  de  la  precision  de  mesure  peuvent  etre 
obtenues  en  installant,  sur  le  bras  du  robot,  des  accelerometres  pour  la  mesure  de 
1’ acceleration  absolue. 

La  realisation  du  systeme  de  controle,  comme  par  example  le  systeme  de  controle 
d’impedance,  a  besoin  de  la  linearisation  dans  l’espace  operationnel  basee  sur  l’approche  de 
1’evaluation  du  couple.  Beaucoup  de  travail  a  ete  fait  pour  1’ amelioration  de  l’approche  de 
revaluation  du  couple  pour  le  controle  du  mouvement  du  robot  en  utilisant  une  variete  de 
methodes  d’ identification  des  parametres,  en  modelisant  la  dynamique  inconnue  et  en 
developpant  des  systemes  de  controle  plus  robuste.  En  general,  revaluation  analytique  du 
couple  a  ete  souvent  consideree  comme  trop  difficile  du  point  de  vue  du  poids  de  calculs  pour 
la  realisation  d’un  systeme  de  control  en  temps  reel. 

Dans  cet  article,  des  ameliorations  supplementaires  au  systeme  de  controle  predictif,  basees 
sur  un  modele  dans  l’espace  operationnel,  sont  proposees  en  utilisant  diverses  methodes 
efficaces  du  point  de  vue  de  l’effort  de  calcul.  Au  debut,  un  observateur  de  la  perturbation  est 
employe  pour  obtenir  une  approximation  de  la  valeur  du  couple  de  linearisation  dans  l’espace 
des  articulations.  L’ observateur  de  la  perturbation  est  base  sur  une  combinaison  des  methodes 
du  ddlai  en  temps  et  du  trajet  de  1’acceleration  pour  estimer  le  couple  non-inertial. 

L’ observateur  de  la  perturbation  a  besoin  de  1’ estimation  de  1’ acceleration  de  1’ articulation 
obtenue  a  partir  des  mesures  et  des  calculs.  Le  calcul  de  l’acceleration  par  differentiation 
numerique  de  la  position  de  l’articulation,  disponible  a  partir  des  capteurs  de  haute  resolution 
de  la  position  de  1’ articulation.  L’acceleration  de  l’articulation  peut  etre  obtenue  par  calculs 
cinematiques,  en  utilisant  les  mesures  de  l’acceleration  absolue.  La  combinaison  des  signaux 
de  l’accelerometre  de  l’outil  du  robot  et  des  signaux  des  capteurs  de  position  des  articulations 
peut  donner  une  estimation  de  1’ acceleration  avec  une  bande  passante  plus  elevee  que  chacun 
separement. 

Pour  ameliorer  la  precision  et  la  bande  passante  des  signaux,  un  Filtre  Kalman  Elargie  est 
utilise  pour  la  fusion  des  capteurs  multiples.  L’algorithme  de  controle  en  temps-reel  a  ete, 
dans  le  passe,  base  sur  l’hypothese  que  la  secousse  est  representee  comme  un  processus  de 
bruit  blanc  avec  moyenne  zero.  En  realite,  les  accelerations  varient  en  temps  pendant  le 
mouvement  du  bras  et  l’hypothese  de  moyenne  zero  n’est  pas  correcte,  en  particular  pendant 
des  periodes  d’ acceleration  rapide.  Dans  ce  rapport,  une  approche  de  commande  predictive 
basee  sur  un  module  est  employee  pour  le  precalcul  de  la  secousse  du  prochain  pas  de  temps 
d’une  telle  fa§on  que  la  composante  restante  peut  etre  representee  comme  un  bruit  blanc 
gaussiene.  Des  resultats  d’experience  illustrent  l’efficacite  de  l’approche  proposee  (Filtre 
Kalman  Elargie)  pour  la  fusion  des  capteurs. 


Jassemi-Zargani,  R.;  Necsulescu,  D.  2001  Extended  Kalman  Filter  Sensor  Fusion  signals  of 
Nonlinear  Dynamic  Systems.  DREO  TR  2001-155,  Centre  de  recherches  pour  la  defense 
Ottawa. 
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1.  Introduction 


Extensive  research  has  been  carried  for  the  purpose  of  achieving  accurate  operational 
space  sensing  and  motion  control  of  robot  arms.  S.  Komada  and  K.  Ohinishi  [1] 
proposed  a  first  order  lag  filter  as  a  perturbation  observer  for  the  estimation  of  the 
unknown  dynamics  terms  used  for  linearization.  T.C.  Hsia  [2]  and  K.  Youcef-Toumi  [3] 
presented  a  similar  method  called  the  time-delay  method,  for  obtaining  the 
compensation  terms.  Hsia  [2]  suggested  a  simple  approach  which  basically  uses  the 
input  and  output  torques  of  robot  joints.  For  short  time  delay,  the  difference  of  these 
torques  is  added  to  the  next  torque  input;  this  is  feasible  as  long  as  the  sampling  time  is 
very  small.  Robust  controllers  were  designed  to  overcome  the  effect  of  some  of  the 
unknown  dynamics  terms.  Adaptation  of  an  impedance  controller  is  proposed  by  W.S. 

Lu  [4],  Also,  an  integral  term  for  improving  the  robustness  of  impedance  controller  was 
considered  by  G.J.  Liu  and  A.  A.  Goldenberg  [5].  Perturbation  observers  require  the  joint 
acceleration  feedback  signal  and  joint  accelerations  can  be  measured  or  calculated.  The 
calculation  by  numerical  derivation  of  joint  position  assumes  that  high  resolution  joint 
sensors  are  available.  Because  of  high  frequency  noise  and  other  numerical  problems, 
this  signal  requires  a  low  pass  filter.  Combining  the  end-effector  accelerometers  and 
joint  position  sensors  can  give  an  acceleration  estimate  with  higher  bandwidth  than 
each  separately.  Sensor  fusion  between  two  sensors  (accelerometers  and  resolvers)  has 
been  proposed  in  [6,7]. 

The  contributions  to  the  integration  of  multiple  sensors  into  a  robotic  system  are 
surveyed  in  [8].  A  hieratical  integration  of  multiple  sensors  into  an  existing  system  is 
proposed  and  evaluated.  The  methods  for  sensory  data  integration  are  not  analyzed.  A 
methodological  analysis  of  Kalman  filter  and  set-based  approaches  for  sensor  data 
fusion  is  presented  in  [9],  Experimental  evaluation  leads  to  the  conclusion  that  the 
performance  of  each  approach  is  problem  dependent.  In  the  methodological  analysis  of 
paper  [9]  and  in  books  [10]  and  [1 1],  the  data  fusion  refer  mainly  to  complex  static 
(geometric)  environmental  data  while  the  data  fusion  problem  in  this  report  refers  to  less 
complex  but  time  varying  (kinematics)  data. 

The  literature  review  shows  that  at  the  present  time  the  estimation  of  kinematic  state 
variables  of  a  dynamic  model  using  data  fusion  from  various  sensors  has  been 
approached  in  an  elaborate  form  only  using  Extended  Kalman  Filter  method  [12-13]. 
This  method  is  proposed  for  the  data  fusion  for  state  estimation  of  manipulators 
considered  in  this  report. 

A  complete  3D  virtual  reality  model  for  robotic  systems  is  useful  for  motion 
visualization,  however,  for  the  operational  space  robot  arm  motion  control  an  abstract 
world  model,  containing  only  joint  and  Cartesian  motion  variables,  is  sufficient  [7,  14]. 
A  proposed  Kalman  filter  for  obtaining  an  accelerometer  signal  was  based  on  a 
kinematic  model  with  a  jerk  assumed  equivalent  to  a  white  noise  source  [15].  This 
paper  presents  a  model  predictive  control  approach  for  replacing  the  pure  white  noise 
jerk  assumption  by  a  more  realistic  model  containing  a  model-based  predicted  part  and  a 
Gaussian  white  noise  part. 
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2.  Computational  Efficiency  of  Impedance 
Controller  Schemes _ 

Figure  1  shows  a  standard  two-part  impedance  control  scheme  consisting  of  a  Cartesian 
decoupling  part  and  a  linear  control  law  part  [7].  The  Cartesian  decoupling  part  is 
described  by: 

t  =  J T  0 9){MX  (0)X  +  Vx  (0, 0)  +  Gx  (0)  +  Fx  (0, 0)  +  Fext )  (1) 

and  contains  the  components  [M  x(0),Vx(0,0),Gx(0),Fx(0,0)  ],  which  contain  both 
kinematic  and  dynamic  parameters  of  the  robot  [8]: 

Mx(0)  =  J-T(0)M(0)J-l(0) 

Vx  (0, 0)  =  J~T  (0)[V(0, 0)~M  (0)J  A  (0)J(0)0] 

Gx(0)  =  J-t(0)G(0 ) 

Fx(0,0)  =  rT(0)Tn(0,0 ) 

where: 

0  the  vector  of  angular  joint  positions 

0  the  vector  of  angular  joint  velocities 

M x{0)  inertia  matrix  in  Cartesian  space 
M  (0)  inertia  matrix  in  joint  space 

Vx  (0, 0)  the  vector  of  centrifugal  and  Coriolis  terms  in  Cartesian  space 

V ( 0 , 0)  the  vector  of  centrifugal  and  Coriolis  terms  in  joint  space 

Gx  (0)  the  vector  gravity  term  in  Cartesian  Space 

G{0 )  the  vector  gravity  term  in  joint  space 

Fx  (0,0)  the  vector  friction  term  in  Cartesian  space 

Tn  (0, 0)  the  vector  of  friction  term  in  joint  space 
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Figure  1.  Two-part  Impedance  controller 


Figure  2  shows  a  three-part  control  scheme  in  which  the  nonlinear  compensator  is 
further  split  into  two  parts,  a  joint  decoupling  scheme  and  the  Cartesian  decoupling 
scheme  for  a  joint  decoupled  robot  arm  [7],  In  the  later  case,  the  Cartesian  decoupling 
scheme  contains  only  the  kinematics  equation: 


ec  =  j~\x(c)  -id)  O) 

while  the  joint  decoupling  scheme  contains  all  the  joint  dynamics  equations  given  by 
the  joint  torque  equation: 

r  =  M{9)9(c)  +Ar  (4) 


where  the  non-inertial  dynamic  terms  are: 

A  r  =  V(9, 9)  +  G(9 )  +  rn  (9, 9)  +  JT  (9) Fexl  (5) 


A  comparison  of  computational  requirements  for  two  and  three-part  impedance 
controllers  is  presented  in  Table  1.  Computation  of  Part  (a),  (Linear  Cartesian  Control 
Law),  and  external  force  (fext)  's  not  considered,  because  they  are  the  same  in  both 

controllers.  The  results  from  Table  1  show  that  the  three-part  controller  has  a  better 
computational  efficiency  than  the  two-paTt  controller.  In  this  paper  the  impedance 
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controller  is  based  on  the  three-part  control  scheme  because  the  methods  proposed  use 
the  explicit  joint  space  linearzation  scheme. 


Figure  2.  Three-part  Impedance  controller 


Table  1.  Comparison  of  two-part  and  three-part  impedance  controllers 


II  Description 

Two-Part 

Impedance 

Controller 

Three-Part 

Impedance 

Controller 

Calculation  of  the  Inverse  of  the 
Jacobian 

1 

i 

Calculation  of  the  Transpose  of 
Inverse  of  the  Jacobian 

1 

0 

Calculation  of  the  Derivative  of 
the  Jacobian 

1 

1 

Use  of  the  Inverse  of  the  Jacobian 

2 

1 

Use  of  the  Transpose  of  Inverse 
of  the  Jacobian 

4 

0 

Use  of  the  Derivative  of  the 
Jacobian 

1 

1 

Multiplication:  Matrix-Matrix 

2 

0 

Multiplication:  Matrix-Vector 

8 

3 

|  Addition:  Vector- Vector 

,  4 

4 

4 
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3.  Acceleration  Perturbation  Observer 


The  dynamic  model  for  a  rigid  link  robot  arm  is  given  by: 

T  =  M(0)0  +  At  (6) 

where  At  is  given  by  equation  (5).  The  non-inertial  dynamic  part  At  of  the  joint 
decoupling  scheme  can  be  estimated  using  a  perturbation  observer  [1-3].  Equation  (6) 
can  also  be  written  as: 

M  (0)T  =  0  +  M~'  (0)At  (7) 

Expanding  the  last  term  as: 

A0  =  M'1(0)At  (8) 

equation  (5)  gives,  for  the  time  t  —  At , 

A  0(t  -At)  =  M  ~ 1  ( 0(t  -  A t))T(t  -  At)  -  0(t  -  At)  (9) 

Given,  that  in  this  work,  accelerations,  not  torques,  are  measured,  the  last  equation  is 
used  as  an  indirect  observer  of  the  non-inertial  terms,  At  [7].  For  a  relatively  short  At 
the  non-inertial  torque  terms,  At  ,  and  inertia  matrix  can  be  assumed  constant: 


At(0  *  At(t-At)  (10) 

M  (0(t))  ~  M  (0(t  -  At))  (11) 

such  that: 

A  0(0  -  A0(t  -  At)  (12) 

Using  At  from  equation  (5)  in  equation  (8)  gives: 

A0  =  M  (0)[V(0,0)  +  Tn  (0,0)  +  G(0)  +  JT(0)Fexl  ]  (13) 


This  confirms  that  non-inertial  torque  terms  can  be  indirectly  estimated  using  an 
acceleration  observer  for  A0 .  After  equating  0  with  the  computed  acceleration  0{c) , 
equation  (6)  and  (8)  give: 

T  =  A/  (0)(0(c)  +  A0)  (14) 
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Finally,  equation  (9), (12)  and(14)  result  in  a  perturbation  observer  for  A6  : 

A0(t)  «  0(c)  (t  -  At)  +  A 0(t  -  At)  -  0(t  -  At)  (15) 

The  block  diagram  shown  in  Figure  3  details  the  impedance  controller  using  a 
perturbation  observer  for  Ad ,  i.e.  indirectly  for  non-inertial  torque  terms. 


Figure  3.  Impedance  controller  with  a  perturbation  observer 


In  Figure  3,  equation  (15)  is  used  to  obtain  the  input  for  the  Time  Delay  block  and 
equation  (14)  gives  the  torque  command  of  the  joint  decoupling  scheme. 
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4.  Joint  Acceleration  Measurement  and 
Estimation 


For  the  method  presented  in  the  previous  section,  joint  acceleration  has  to  be  accurately 
estimated.  There  are  different  concurrent  ways  to  obtain  this  estimation;  sensor  fusion 
was  selected  for  to  obtain  estimations  based  on  the  results  from  relative  link  positions 
and  absolute  link  acceleration  measurements.  This  choice  of  sensors  is  a  result  of  the 
availability  of  sensors  for  the  measurement  of  relative  angular  displacement  of  robot 
links  (using  high  resolution  resolvers  or  optical  encoders)  and  absolute  link  acceleration 
using  accelerometers.  By  numerical  derivation  of  joint  position,  relative  acceleration 
can  be  obtained  using  backward  approximation: 


m  = 


0(t-2At)-20(t-At)  +  0(t) 
2A  t 


(16) 


Absolute  accelerations  can  be  measured  using  accelerometers  installed  on  the  robot 
links. 

Joint  accelerations  are  obtained  from  link  accelerometer  signals  and  the  estimated  joint 
speed  using  the  kinematic  equation  of  the  accelerations.  For  6 ,  the  vector  of  the  joint 
angular  positions,  and  x ,  the  vector  of  the  Cartesian  accelerations,  joint  acceleration 

0  can  be  calculated  from  the  kinematic  equation  for  a  rigid  link  robot  with  revolute 
joints: 

0  =  J~'(0)[x-j(0)0]  (17) 

where: 

0  joint  velocities  vector 

J  Jacobian  matrix 
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5.  Sensor  Fusion  Using  Extended  Kalman  Filter 
for  Acceleration  Evaluation _  _ 

A  suitable  method  for  sensor  fusion  of  accelerometer  and  resolver  outputs  is  the 
Extended  Kalman  Filter  ( EKF).  EKF  proved  suitable  for  real  time  estimation  of 
kinematic  variables  given  the  presence  of  the  noise  and  the  non-linearity  of  the  robot 
arm  dynamics.  For  nonlinear  systems,  an  EKF  can  estimate  the  states  of  non-linear 
systems  using  a  linearized  approximation  based  on  current  state  estimate  [12].  Figure  4 

shows  the  block  diagram  of  the  robot  controller  using  EKF  for  the  estimation  of  6  and 

0. 


For  a  two  degree-of-freedom  revolute  robot  arm,  given  joint  one  and  two  angular 
positions#!  and  02 ,  the  states  are  defined  as: 
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*1  =0i 

x2  =  02 

*3  =  *1  =  01 
*4  =  *2  =  (92 
x5  =  jc3  = 
x6  =  i4  =  @2 


(18) 


The  jerks,  i5,and,  i6 ,  are  assumed  time  independent  white  noise  processes  V{  and 
V2 ,  respectively.  State  equations  are: 


(19) 


The  noisy  measurements,  y, ,  and,  y2 ,  of  angular  positions  9X  and  92  are  modeled  by 
the  following  measurement  equations: 


yt  *  x,  +  Wj 

y2  «  x2  +  w2 


(20) 


The  noisy  measurements,  y3 ,  and  y4 ,  of  the  Cartesian  accelerations  are  used  in 
measurement  equations  based  on  the  kinematic  equation. 

x  =  J(0)0  +  j(0)0  (21) 


or,  using  above  defined  states, 


?3 

T4 


=  J(xvx2) 


+  j(xl,x2,x3,x4) 


where  [7,8]: 


J(xl,X2)  = 


Jn  Jl2 

J2l  J  22 


(22) 
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r  11 


12 


J2l  J  22. 


J  (Xj  ,  X2  ,  X2  y  Xq  )  | 

State  equations  in  matrix  form  are: 

x  =  Ax  +  BV 
y  =  Cx  +  w 

where: 


C  = 


'0 

0 

1 

0 

0 

o' 

'0 

0 

0 

0 

0 

o' 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

B 

— 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

_0 

0 

0 

0 

0 

1 

"1 

0 

0 

0 

0 

0  ' 

0 

1 

0 

0 

0 

0 

0 

0 

{u 

J  2 1 

J 

11 

J 12 

0 

0 

^21 

J22 

J 

21 

J 22  _ 

x  =  [x, 

X  =  [Xi 

y  =  [yi 
V  =  [0  o  o  o 
w. 


x3  x4  x5 


x3  x4  x5 


*«] 

*«] 


y 2  y3  y 4]7 


v;  v2y 

w4]r 


(23) 


W  =  [W[  w2 

The  state  equations  of  the  system,  for  short  sampling  time(  At ),  can  be  written  as: 


x(n  + 1)  =  ax(n )  +  bv(n ) 
y(n)  =  cx(n)  +  w(n) 


(24) 


where: 
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1 

0 

T 

0 

0 

o' 

'0 

0 

0 

0 

0 

o' 

0 

1 

0 

T 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

T 

0 

b  ~TB  = 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

T 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

T 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

T 

v(n)  =  V 


5.1  Linearization  of  output  equations 

The  measurement  equations  for)'/  and  y/  are  linear: 


y  i  =  *1  + 


y2=x2+  w2 


(25) 


The  output  equations  for  y?  and  y4  are  nonlinear  and,  in  order  to  apply  EKF,  these 
equations  have  to  be  linearized  .  The  linearized  version  of  the  non-linear  equation  is: 

y(t)  =  h(x(t),u(t),w(t))  (26) 


is  obtained,  by  linearizing  equation  (26)  about  the  current  estimation  x(t)  of  the  state 
vector  ( x (t)  =  x (t),  w(f)  =  0) )  [12],  The  following  linear  equation  is  thus  obtained: 


y(f)  =  h(x(t)Mt),0)  +  H(t)[x(t)-x(t)]  +  G(t)w  (27) 

where: 


H(t)  = 


G(t)  = 


dh(x(t),u(t),w(t )) 
dx(t ) 

dhjycjtXu  (t),w(t)) 

dw(t ) 


The  measurement  equation  for  y?  is  linearized  as  follows  [6,7]: 


(28) 
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73  =  73(*)+ 


9y3 


:  (X  “  X) 


dx'x°x 

=  7,  ,(*)*,  +  jn(x)x 4  +  /n(i)£j  +  7,,  (*)*,,  +  »\ 


ay,,)-  ay,,)  ay,,)  ay,,) 

-  X,  i  -  x4  -t-  ^  x,  -r  x6 

OX 


dx 

dx3 
dx 


dx 

dxA 


dx 


[x-x] 


(29) 


A.^+A^+A.^+a.^ltx-i] 


dx 


dx 


while  the  fourth  measurement  equation  for  y4  is  linearized  as  follows: 

dv 

74  =  74(*)+if1U^*_*) 

dx 

=  721(x)x3  +  j22(x)x 4  +  721(x)x5  +  J22(x)x6  +  W4 
^(■^2l)s  ,  ^(*^22)0  ,  ^(*^21)  c  I  ^(722) 


dx  3 


x,  +  - 


dx  4 


XA  +- 


dx  5 


Xc  +  " 


.'w 

dx 


x-x] 


(30) 


7,,^  +  7„^  +  7,1^  +  7„^  |[x-x] 


21  3x  22  *= 


9x  21  9x 


9x 


The  four  output  equations  can  now  be  represented  by  the  following  linear  discrete  time 
equation: 


y  =  cx(n)  +  w  +  e(n) 

where: 


(31) 


c  = 


1  0  0  0  0  0 

0  1  0  0  0  0 

y3i  y3 2  y33  734  735 

.741  742  7 43  744  745  746 , 


e  = 


1 

0 

•73i 

•741 


0  0  0 

1  0  0 

—  732  7U  —  733  712 


0 

0 


0 

0 


734  7  u  y35  712  y36 


742  7  21  y43  7 22  744 


721  745  722  746. 


where  the  y  terms  are  given  in  [6,7], 
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5.2  Extended  Kalman  Filter  algorithm 

The  Extended  Kalman  Filter  (EKF)  algorithm  uses  the  following  steps  [12]: 
Kalman  Gain  matrix: 


K(n )  =  P(n)~  cT  (n)[c(n)P(n)~  cT  (n)  +  wd]  (32) 

-  State  estimate  update: 

x (n)+  =  x{n)~  +  K(n)(y(n)-c(n)x(n)~  -  e(n ))  (33) 

State  estimate  extrapolation: 

x(n  + 1)"  =  a(n)x(n)+  +  b(n)u  (34) 

Error  covariance  update: 

P(n)+  =  (/  -  K(n)c(n))P(nY  (35) 

Error  covariance  extrapolation: 

P(n  + 1)~  =  a(n)P(n)+  a(n)T  +  Vd  (36) 


The  noises  wd  and  Vd  are  assumed  Gaussian  white  noises,  time  invariant  with  zero 
mean.  The  initial  values  were  chosen  as  follows: 


10'12  0  0  0 

0  10’12  0  0 

0  0  .0.8  0 

0  0  0  0.8 

0 
0 
0 
0 

5  *10“5 
5  *10-5 


(37) 


(38) 
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6.  Experimental  Set-Up 


The  experimental  the  setup  used  for  this  paper  consists  of  a  two  degree-of-freedom 
planar  robot  arm,  where  each  rigid  link  is  driven  by  a  direct  drive  motor  (Figure  5).  All 
motors  are  connected  to  the  dSPACE(digital  Signal  Processing  and  Control 
Engineering)  system,  which  executes  the  control  program  for  robot  motion  [16].  The 
TRACE  software  is  used  to  trace  any  variable  of  the  program  in  real  time  and  runs  on  a 
personal  computer ,  connected  to  dSPACE  by  bus  expansion. 
dSPACE  boards  are: 

DS  1002,  Floating  point  DSP  board  with  TMS320C30, 

DS  3001,  Incremental  Encoder  board, 

DS  2101,  Digital  to  Analog  Conversion  board, 

DS  2002,  Analog  to  Digital  Conversion  board. 


Drivers 


Figure  5.  Experimental  set-up 


14 


DREOTR  2001-155 


7.  Experimental  Results 


7.1  Joint  acceleration  estimation 

Joint  accelerations  for  each  joint  of  the  robot  are  obtained  for  different  cases.  Figure  6 
presents  the  following  results:  curve  (A)  represents  the  results  of  double  numerical 
derivation  (using  equation  (16))  of  the  joint  position  measurements,  curve  (B)  represents 
joint  acceleration  obtained  using  a  kinematic  equation  (17)  based  on  Cartesian 
acceleration  measurements  and  the  derivative  of  joint  position  measurement,  and  curve 
(C)  shows  the  result  of  the  fusion  of  case  (A)  and  (B)  using  the  EKF  algorithm. 


Figure  6.  Experimental  and  estimation  results  for  joint  acceleration,  double  numerical  derivative 
of  joint  position  measurement  using  equation  (16),  joint  acceleration  calculation  using  equation 
(17)  and  accelerometer  signals,  fusion  of  (A)  and  (B)  results  using  EKF. 


7.2  Impedance  control  of  single  arm  using  acceleration 
estimation 


The  significance  of  the  difference  between  these  results  is  based  on  generating  point- 
to-point  robot  motion.  In  the  case  of  perfect  impedance  control,  the  path  generated  for 
the  endpoint  of  the  robot  would  be  a  straight  line.  The  efficiency  of  the  proposed 
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improvements  for  the  impedance  control  can,  consequently,  be  evaluated  based  on 
comparing  actual  generated  path  to  ideal  straight  line  path. 

Figure  7  shows  the  resulting  end-effector  trajectories  for  a  two  degree-of-freedom 
planar  robot  from  the  initial  position(I)  to  the  desired  position(D).  This  robot  has  been 
tested  for  different  cases.  In  case  (a),  all  the  dynamic  terms  have  been  compensated  by 
analytical  computation.  Case(b)  is  the  same  as  (a)  except  for  the  addition  a  joint 
observer  for  compensating  unmodelled  dynamic  terms,  using  estimated  joint 
accelerations  from  endpoint  accelerometer  outputs.  In  case  (c),  PD  (Proportional 
Derivative)  operational  space  control  was  used  without  any  compensation  of  Coriolis, 
centrifugal  or  friction  terms;  in  this  case,  as  shown,  the  robot  arm  could  not  reach  the 
target.  Better  performance  can  be  obtained  by  tuning  the  PD  gains,  but  the  performance 
is  dependent  on  chosen  I  and  D  points.  In  case  (d),  only  joint  observers  are  used  (  with 
no  analytical  computation  for  the  compensation  of  Coriolis,  centrifugal  or  friction 
terms);  end  point  accelerometers  outputs  are  used  again  to  calculate  joint  accelerations. 


Y(m) 


D 

b 

\ 

d 

I 

t 

s 

\  'i 

0  -O 

/ 

1  ( 

)  0 

1  0. 

2  0. 

X(m) 


Figure  7.  Experimental  results  for  end-effector  trajectories  for:  (a)  analytical  computation  of  all 
dynamic  terms,  (b)  joint  observer  plus  analytical  computation  of  all  dynamic  terms,  (c)  PD 
operation  of  space  control,  (d)  same  as  (b)  without  analytical  computation 


Cases  (a),  (b)  and  (d)  show  trajectories  that  are  all  deflected  from  the  ideal  straight  line 
as  a  result  of  various  forms  of  partial  compensation  of  the  dynamic  terms.  The 
conclusion  is  that  all  dynamic  terms  play  a  significant  role  and  consequently  all  terms 
have  to  be  compensated  to  obtain  a  trajectory  as  close  as  possible  to  the  ideal  straight 
line.  Further  improvements  are  obtained  in  impedance  control  by  the  use  of  an  EKF  for 
sensor  fusion. 
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7.3  Experimental  results  of  robot  arm  impedance  control 
using  an  Extended  Kalman  Filter  based  on  sensor  fusion 


Figure  8  shows  the  results  when  the  dynamic  terms  are  compensated  by  approximate 
models  and  an  observer.  Curve  (a)  is  the  trajectory  of  the  end-effector  using  acceleration 
obtained  as  the  second  derivative  of  the  joint  position.  Curve  (b)  is  the  trajectory  based 
on  estimated  acceleration  using  an  EKF  and  an  approximation  to  a  dynamic 
compensator.  The  results  show  that  the  trajectory  that  is  generated  by  case  (b)  is  closer 
to  a  straight  line  than  that  of  case  (a).  These  results  suggest  that  an  EKF-based  approach 
will  significantly  improve  the  performance  of  robot  motion  control. 


Figure  8.  End-effector  trajectory  of  robot  arm  based  on  two  methods:  (a)  second  derivative  of 
joint  positions  and  (b)  EKF  in  parallel  with  approximate  compensation  models  of  dynamic 

terms 
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8.  Conclusions  and  Recommendations 


Non-inertial  terms  in  the  computed  torque  approach  used  in  Impedance  control  can  be 
estimated  indirectly  using  an  acceleration  perturbation  observer.  Improved  acceleration 
estimates  can  be  obtained  by  fusing  signals  from  joint  position  and  link  acceleration 
sensors.  The  experimental  results  prove  that  the  perturbation  observer  based  on  sensor 
fusion  can  improve  the  performance  of  an  impedance  controller  for  the  trajectory 
generation  of  a  robot  arm  and  that  an  almost  straight  line  trajectory  is  generated.  These 
results  indicate  that  for  obtaining  a  more  accurate  acceleration  signal,  the  Extended 
Kalman  Filter  algorithm  is  an  efficient  sensor  fusion  method.  The  controller  is  able  to 
compensate  dynamic  terms,  including  centrifugal,  Coriolis,  and  friction  terms. 
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